#include <iostream>
#include <math.h>
#define Turntable_OFFSET 20
#define SARM_OFFSET 30 // 小臂
#define BARM_OFFSET 25
#define Grippers_OFFSET 9

#define BASE_HEIGHT 6
#define L1 8 // 大臂长度
#define L2 8.5

struct ServoDegree
{
    double theta_1, theta_2, theta_3;
};

ServoDegree GetServoDegree(double rx, double ry, double rz)
{
    ServoDegree aim;
    aim.theta_1 = atan2f(ry, rx); // 转台角度
    double xe = rx - Grippers_OFFSET * cos(aim.theta_1);
    double ye = ry - Grippers_OFFSET * sin(aim.theta_1);
    double x_e_2 = rz + 2.5 - BASE_HEIGHT;              // 末端相对于转台的x轴，高度轴
    double y_e_2 = sqrt(pow(xe, 2) + pow(ye, 2)); // 末端相对于转台的y轴，向前的轴。(0,0)是转台圆心
    // double z_e_2 = 0;                             // 末端相对于转台的z轴，向左右的轴。
    double L3 = sqrt(pow(x_e_2, 2) + pow(xe, 2) + pow(ye, 2));
    double theta_a = acos((L1 * L1 + L3 * L3 - L2 * L2) / (2 * L1 * L3));
    double theta_b = atan2(y_e_2, x_e_2);
    aim.theta_2 = theta_b - theta_a; // 大臂角度 竖直往前是正
    double theta_d = acos((L1 * L1 + L2 * L2 - L3 * L3) / (2 * L1 * L2));
    aim.theta_3 = theta_d - (3.1415 / 2) - aim.theta_2; // 小臂角度 水平向上为正
                                                        // 以上为实际角度
    aim.theta_1 *= 57.3;
    // aim.theta_2 = 90 - aim.theta_2 * 57.3  + BARM_OFFSET;
    // aim.theta_3 = -(aim.theta_3 * 57.3) + SARM_OFFSET;
    aim.theta_2 = aim.theta_2 * 57.3 ;
    aim.theta_3 = -(aim.theta_3 * 57.3) ;

    // if (aim.theta_1 < 0 || aim.theta_1 > 180 || aim.theta_3 < 5 || aim.theta_3 > 80 || aim.theta_2 < 20 || aim.theta_2 > 120)
    // {
    //     aim.theta_2 = 0;
    //     return aim;
    // }
    return aim;
}

int main()
{
    ServoDegree aim = GetServoDegree(0, 15, 8);
    printf("%f,%f,%f\n", aim.theta_1 , aim.theta_2 , aim.theta_3 );
}

